%%%-------------------------------------------------------------------
%%% @author Rain
%%% @doc 帧同步计算相关
%%% @end
%%%-------------------------------------------------------------------
-module(ls_calc).
-author("Rain").
-include("elock_step.hrl").

%% API
-export([look_rotation/2]).
-export([add_vector/2, subtract_vector/2]).

add_vector(#ls_vector{x = OldX, y = OldY, z = OldZ}, #ls_vector{x = NewX, y = NewY, z = NewZ}) ->
    SumX = NewX + OldX,
    SumY = NewY + OldY,
    SumZ = NewZ + OldZ,
    #ls_vector{x = SumX, y = SumY, z = SumZ}.

subtract_vector(#ls_vector{x = OldX, y = OldY, z = OldZ}, #ls_vector{x = NewX, y = NewY, z = NewZ}) ->
    DiffX = NewX - OldX,
    DiffY = NewY - OldY,
    DiffZ = NewZ - OldZ,
    #ls_vector{x = DiffX, y = DiffY, z = DiffZ}.

% 向量归一化
vector_normalize(Vector) ->
    Magnitude = math:sqrt(vector_dot(Vector, Vector)),
    #ls_vector{x = Vector#ls_vector.x / Magnitude,
        y = Vector#ls_vector.y / Magnitude,
        z = Vector#ls_vector.z / Magnitude}.

% 向量叉积
vector_cross(V1, V2) ->
    #ls_vector{x = (V1#ls_vector.y * V2#ls_vector.z) - (V1#ls_vector.z * V2#ls_vector.y),
        y = (V1#ls_vector.z * V2#ls_vector.x) - (V1#ls_vector.x * V2#ls_vector.z),
        z = (V1#ls_vector.x * V2#ls_vector.y) - (V1#ls_vector.y * V2#ls_vector.x)}.

% 向量点积
vector_dot(V1, V2) ->
    (V1#ls_vector.x * V2#ls_vector.x) + (V1#ls_vector.y * V2#ls_vector.y) + (V1#ls_vector.z * V2#ls_vector.z).

% 向量获取 x 分量
vector_get_x(Vector) ->
    Vector#ls_vector.x.

% 向量获取 y 分量
vector_get_y(Vector) ->
    Vector#ls_vector.y.

% 向量获取 z 分量
vector_get_z(Vector) ->
    Vector#ls_vector.z.

look_at(Forward, Upwards) ->
    ZAxis = vector_normalize(Forward),
    XAxis = vector_cross(Upwards, ZAxis),
    YAxis = vector_cross(ZAxis, XAxis),

    Matrix = #{
        {1, 1} => vector_get_x(XAxis),
        {2, 1} => vector_get_x(YAxis),
        {3, 1} => vector_get_x(ZAxis),
        {1, 2} => vector_get_y(XAxis),
        {2, 2} => vector_get_y(YAxis),
        {3, 2} => vector_get_y(ZAxis),
        {1, 3} => vector_get_z(XAxis),
        {2, 3} => vector_get_z(YAxis),
        {3, 3} => vector_get_z(ZAxis)
    },
    Matrix.

look_rotation(Forward, Upwards) ->
    create_from_matrix(look_at(Forward, Upwards)).

create_from_matrix(Matrix) ->
    #{
        {1, 1} := M11,
        {2, 2} := M22,
        {3, 3} := M33,
        {2, 3} := M23,
        {3, 2} := M32,
        {3, 1} := M31,
        {1, 3} := M13,
        {1, 2} := M12,
        {2, 1} := M21
    } = Matrix,
    Num8 = M11 + M22 + M33,
    if
        Num8 > 0 ->
            Num = math:sqrt(Num8 + 1),
            #ls_quaternion{
                x = math:sqrt(0.5) * Num,
                y = (M23 - M32) * 0.5 / Num,
                z = (M31 - M13) * 0.5 / Num,
                w = (M12 - M21) * 0.5 / Num};
        M11 >= M22 andalso M11 >= M33 ->
            Num7 = math:sqrt((1 + M11 - M22 - M33)),
            #ls_quaternion{
                x = 0.5 * Num7,
                y = (M12 + M21) * 0.5 / Num7,
                z = (M13 + M31) * 0.5 / Num7,
                w = (M23 - M32) * 0.5 / Num7};
        M22 > M33 ->
            Num6 = math:sqrt((1 + M22 - M11 - M33)),
            #ls_quaternion{
                x = (M21 + M12) * 0.5 / Num6,
                y = 0.5 * Num6,
                z = (M32 + M23) * 0.5 / Num6,
                w = (M31 - M13) * 0.5 / Num6};
        true ->
            Num5 = math:sqrt((1 + M33 - M11 - M22)),
            #ls_quaternion{
                x = (M31 + M13) * 0.5 / Num5,
                y = (M32 + M23) * 0.5 / Num5,
                z = 0.5 * Num5,
                w = (M12 - M21) * 0.5 / Num5}
    end.